/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "stdafx.h"
#include "GrabRob_3_RRR.h"

namespace GrabRobModel
{
	using namespace std;
	using namespace Robotic::Math;
	using namespace Robotic::Mechanism;

	GrabRob_3_RRR::GrabRob_3_RRR()
	{
		_dof = 3;
		_links = new Link*[_dof];

		//initialize link 1
		Link_DH* pLink1 = new Link_DH();
		DHParameter dhParam1;
		dhParam1.theta = 0;
		dhParam1.d = 1;
		dhParam1.a = 0;
		dhParam1.alpha = PI / 2;
		pLink1->Set_DHParam(dhParam1);
		pLink1->Set_JointType(Revolute);
		PhysicsParameter phyParam1;
		phyParam1.Mass = 1;
		phyParam1.MassCenter = Vector3(0,-0.05,0);
		phyParam1.Ic = Inertia(0.5,0.5,0.5,0,0,0);
		phyParam1.GearRatio = 1;
		phyParam1.GearTransEffi = 1;
		phyParam1.Jm = 1;
		phyParam1.B = 0.1;
		phyParam1.Cf = 0.1;
		phyParam1.MotorTorqueMax = 0;
		phyParam1.MotorTorqueMin = 0;
		pLink1->Set_PhysicsParam(phyParam1);
		_links[0] = pLink1;

		//initialize link 2
		Link_DH* pLink2 = new Link_DH();
		DHParameter dhParam2;
		dhParam2.theta = 0;
		dhParam2.d = 0;
		dhParam2.a = 1;
		dhParam2.alpha = 0;
		pLink2->Set_DHParam(dhParam2);
		pLink2->Set_JointType(Revolute);
		PhysicsParameter phyParam2;
		phyParam2.Mass = 1;
		phyParam2.MassCenter = Vector3(-0.05,0,0);
		phyParam2.Ic = Inertia(0.5,0.5,0.5,0,0,0);
		phyParam2.GearRatio = 1;
		phyParam2.GearTransEffi = 1;
		phyParam2.Jm = 1;
		phyParam2.B = 0.1;
		phyParam2.Cf = 0.1;
		phyParam2.MotorTorqueMax = 0;
		phyParam2.MotorTorqueMin = 0;
		pLink2->Set_PhysicsParam(phyParam2);
		_links[1] = pLink2;

		//initialize link 3
		Link_DH* pLink3 = new Link_DH();
		DHParameter dhParam3;
		dhParam3.theta = 0;
		dhParam3.d = 0;
		dhParam3.a = 1;
		dhParam3.alpha = 0;
		pLink3->Set_DHParam(dhParam3);
		pLink3->Set_JointType(Revolute);
		PhysicsParameter phyParam3;
		phyParam3.Mass = 1;
		phyParam3.MassCenter = Vector3(-0.05,0,0);
		phyParam3.Ic = Inertia(0.5,0.5,0.5,0,0,0);
		phyParam3.GearRatio = 1;
		phyParam3.GearTransEffi = 1;
		phyParam3.Jm = 1;
		phyParam3.B = 0.1;
		phyParam3.Cf = 0.1;
		phyParam3.MotorTorqueMax = 0;
		phyParam3.MotorTorqueMin = 0;
		pLink3->Set_PhysicsParam(phyParam3);
		_links[2] = pLink3;

	}

	GrabRob_3_RRR::~GrabRob_3_RRR() 
	{
	}

	bool GrabRob_3_RRR::InverseKine_Pose(const Frame3& endFrame, std::vector<ColumnVector>& retJointValues)const
	{
		std::vector<ColumnVector>& jointValues = std::vector<ColumnVector>();

		double q1, q2, q3;
		double Xc = endFrame.Translation[0];
		double Yc = endFrame.Translation[1];
		double Zc = endFrame.Translation[2];
		DHParameter dhParam1 = ((Link_DH*)_links[0])->Get_DHParam();
		DHParameter dhParam2 = ((Link_DH*)_links[1])->Get_DHParam();
		DHParameter dhParam3 = ((Link_DH*)_links[2])->Get_DHParam();
		double d = 0;

		q1 = atan2(Yc, Xc);

		double D = (Xc*Xc + Yc*Yc - d*d + (Zc-dhParam1.d)*(Zc-dhParam1.d) - dhParam2.a*dhParam2.a - dhParam3.a*dhParam3.a) / (2*dhParam2.a*dhParam3.a);
		double E = Xc*Xc + Yc*Yc - d*d;
		if(D < 0)
			return false;
		if(E < 0)
			return false;

		E = sqrt(E);
		if(D < 1)
		{
			ColumnVector qVector(3);

			double temp = sqrt(1 - D*D);
			q3 = atan2(temp, D);
			q2 = atan2(Zc - dhParam1.d, E) - atan2(dhParam3.a*sin(q3), dhParam2.a + dhParam3.a*cos(q3));
			qVector[0] = q1;
			qVector[1] = q2;
			qVector[2] = q3;
			jointValues.push_back(qVector);
			if(q1 > 0)
				qVector[0] = -PI + q1;
			else
				qVector[0] = PI + q1;
			if(q2 > 0)
				qVector[1] = PI - q2;
			else
				qVector[1] = -PI - q2;
			qVector[2] = -q3;
			jointValues.push_back(qVector);


			q3 = atan2(-temp, D);
			q2 = atan2(Zc - dhParam1.d, E) - atan2(dhParam3.a*sin(q3), dhParam2.a + dhParam3.a*cos(q3));
			qVector[0] = q1;
			qVector[1] = q2;
			qVector[2] = q3;
			jointValues.push_back(qVector);
			if(q1 > 0)
				qVector[0] = -PI + q1;
			else
				qVector[0] = PI + q1;
			if(q2 > 0)
				qVector[1] = PI - q2;
			else
				qVector[1] = -PI - q2;
			qVector[2] = -q3;
			jointValues.push_back(qVector);
		}
		else if(D == 1)
		{
			jointValues = std::vector<ColumnVector>(1);
			ColumnVector qVector(3);
			q3 = 0;
			q2 = atan2(Zc - dhParam1.d, E) - atan2(dhParam3.a*sin(q3), dhParam2.a + dhParam3.a*cos(q3));
			qVector[0] = q1;
			qVector[1] = q2;
			qVector[2] = q3;
			jointValues.push_back(qVector);
			if(q1 > 0)
				qVector[0] = -PI + q1;
			else
				qVector[0] = PI + q1;
			if(q2 > 0)
				qVector[1] = PI - q2;
			else
				qVector[1] = -PI - q2;
			qVector[2] = -q3;
			jointValues.push_back(qVector);
		}
		else
		{
			return false;
		}

		retJointValues = GetAllSolution(jointValues);

		if(retJointValues.size() <= 0)
			return false;
		else
			return true;
	}

	//bool GrabRob_3_RRR::InverseKine_Vel(const ColumnVector& jointValues, const Velocity& endFrameVel, ColumnVector& retJointVels)const
	//{
	//	if(jointValues.GetCount() != this->_dof)
	//		throw(new exception(" Argument jointValues's count must be equal to dof."));

	//	Matrix jaco = Jacobian(jointValues);
	//	Matrix jaco2 = jaco.GetSubMatrix(0,2,0,2);

	//	if(jaco2.Determinant() == 0)
	//		return false;

	//	Matrix iJaco2 = jaco2.Inverse();
	//	retJointVels = iJaco2 * endFrameVel.LinearVel;

	//	/*if(_dof < 6)
	//	{
	//		Matrix jT = jaco.Transpose();
	//		Matrix jT_j = jT*jaco;
	//		if(jT_j.Determinant() == 0)
	//			return false;

	//		retJointVels = jT_j.Inverse()*jT*endFrameVel;
	//	}
	//	else if(_dof == 6)
	//	{
	//		if(jaco.Determinant() == 0)
	//			return false;

	//		retJointVels = jaco.Inverse()*endFrameVel;
	//	}
	//	else
	//	{
	//		Matrix jT = jaco.Transpose();
	//		Matrix j_jT = jaco*jT;
	//		if(j_jT.Determinant() == 0)
	//			return false;

	//		retJointVels = jT*j_jT.Inverse()*endFrameVel;
	//	}*/

	//	return true;
	//}

	//bool GrabRob_3_RRR::InverseKine_Acc(const ColumnVector& jointValues, const ColumnVector& jointVels, const Acceleration& endFrameAcc, ColumnVector& retJointAccs)const
	//{
	//	if((jointValues.GetCount() != this->_dof) ||(jointVels.GetCount() != this->_dof) )
	//		throw(new exception(" Argument jointValues's and jointVels's count must be equal to dof."));

	//	Matrix jaco = Jacobian(jointValues);
	//	Matrix jaco2 = jaco.GetSubMatrix(0,2,0,2);

	//	Matrix jaco_dot = Jacobian_dot(jointValues,jointVels);
	//	Matrix jaco_dot2 = jaco_dot.GetSubMatrix(0,2,0,2);
	//	ColumnVector eFA = endFrameAcc.LinearAcc - jaco_dot2*jointVels;

	//	if(jaco2.Determinant() == 0)
	//		return false;

	//	retJointAccs = jaco2.Inverse()*eFA;


	//	/*if(_dof < 6)
	//	{
	//		Matrix jT = jaco.Transpose();
	//		Matrix jT_j = jT*jaco;
	//		if(jT_j.Determinant() == 0)
	//			return false;

	//		retJointAccs = jT_j.Inverse()*jT*eFA;
	//	}
	//	else if(_dof == 6)
	//	{
	//		if(jaco.Determinant() == 0)
	//			return false;

	//		retJointAccs = jaco.Inverse()*eFA;
	//	}
	//	else
	//	{
	//		Matrix jT = jaco.Transpose();
	//		Matrix j_jT = jaco*jT;
	//		if(j_jT.Determinant() == 0)
	//			return false;

	//		retJointAccs = jT*j_jT.Inverse()*eFA;
	//	}*/

	//	return true;
	//}


}